#include "mapfilter_zwk.h"

//ros ::NodeHandle nh;

MapFilter_Zwk::MapFilter_Zwk()
{
    BLACK=0;
    WHITE=255;
    GRAY=205;
    OTHER = 100;

    inflate_num = 1;
    min_area = 20;
    max_area = 9999;
    maxLimitedNum_black = 2;
    maxLimitedNum_white = 10;
    printf("**********************************************\n");
    printf("inflate_num,min_area,max_area,maxLimitedNum_black,maxLimitedNum_white:%d,%d,%d,%d,%d\n",
             inflate_num, min_area, max_area, maxLimitedNum_black, maxLimitedNum_white);
}

MapFilter_Zwk::~MapFilter_Zwk()
{}

bool MapFilter_Zwk::hasValidDirection(const Mat &img, int x, int y)
{
    if((x>0&&x<img.cols-1)&&(y>0&&y<img.rows-1))
    {
        // 3*3 region have no black pixel neighbor
        for(int i=-1;i<2;++i)
        {
            for(int j=-1;j<2;++j)
            {
                if(i==0&&j==0)
                    continue;
                int pixelValue=(int)img.at<uchar>(y+i,x+j);
                if(pixelValue==BLACK)
                    return true;
            }
        }
        //3*3 region have to white->black->gray
        if(abs((int)img.at<uchar>(y-1,x-1)-(int)img.at<uchar>(y+1,x+1))>0)
            return true;
        if(abs((int)img.at<uchar>(y-1,x)-(int)img.at<uchar>(y+1,x))>0)
            return true;
        if(abs((int)img.at<uchar>(y-1,x+1)-(int)img.at<uchar>(y+1,x-1))>0)
            return true;
        if(abs((int)img.at<uchar>(y,x+1)-(int)img.at<uchar>(y,x-1))>0)
            return true;
        return false;
    }
    return true;
}

ConnectedEdge* MapFilter_Zwk::BFS(const Mat &img, const Point2D &initPoint, int initPixelVale, unordered_map<int, bool> &isVisited, int stopPixelValue, int maxGrayNums, bool isLimited)
{
    ConnectedEdge* pEdge=new ConnectedEdge;
    pEdge->points.push_back(initPoint);
    int initIndex=initPoint.y*img.rows+initPoint.x;
    isVisited[initIndex]=true;
    queue<Point2D> Q;

    Q.push(initPoint);
    while(!Q.empty())
    {
        Point2D temp=Q.front();
        for(int i=-1;i<2;++i)//y rows  height
        {
            for(int j=-1;j<2;++j)//x cols wide
            {
                if(i==0 && j==0)
                    continue;
                int pixelValue=(int)img.at<uchar>(temp.y+i,temp.x+j);
                if(isLimited && pixelValue==stopPixelValue)
                {
                    delete pEdge;
                    return nullptr;
                }

                if(pixelValue!=initPixelVale)
                    continue;
                int tempIndex=(temp.y+i)*img.rows+temp.x+j;
                unordered_map<int,bool>::iterator iter=isVisited.find(tempIndex);
                if(iter==isVisited.end())
                {
                    isVisited[tempIndex]=true;
                    pEdge->points.push_back(Point2D(temp.x+j,temp.y+i));
                    if(isLimited && pEdge->points.size()>maxGrayNums)
                    {
                        delete pEdge;
                        return nullptr;
                    }
                    Q.push(Point2D(temp.x+j,temp.y+i));
                }
            }
        }
        Q.pop();
    }
    return pEdge;
}

void MapFilter_Zwk::getBinaryImage(Mat &oriImg, Mat &binaryImg)
{
    for(int i=0;i<oriImg.rows;++i)
    {
        for(int j=0;j<oriImg.cols;++j)
        {
            int pixelValue=(int)oriImg.at<uchar>(i,j);
            if(pixelValue!=BLACK)
                binaryImg.at<uchar>(i,j)=WHITE;//covert gray to white, white keep unchanged!
            else{
                if(hasValidDirection(oriImg,j,i))
                    binaryImg.at<uchar>(i,j)=BLACK;
                else{
                    binaryImg.at<uchar>(i,j)=WHITE;
                    oriImg.at<uchar>(i,j)=oriImg.at<uchar>(i-1,j-1);//incorporate invalid isolated points into the background
                }
            }
        }
    }
}

void MapFilter_Zwk::removeLittleColorRegion(Mat &oriImg, Mat &optImg, int colorType)
{
    int preLimitedColor,curLimitedColor,isStoppedColor,isFilledColor,maxLimitedNum;
    if(colorType==WHITE)
    {
//          preLimitedColor=BLACK;
        curLimitedColor=WHITE;
        isStoppedColor=OTHER;
        isFilledColor=GRAY;
        maxLimitedNum=maxLimitedNum_white;
    }else if(colorType==GRAY)
    {
        preLimitedColor=WHITE;
        curLimitedColor=GRAY;
        isStoppedColor=BLACK;
        isFilledColor=WHITE;
        maxLimitedNum=30;
    }
    else if(colorType==BLACK)
    {
        preLimitedColor=WHITE;
        curLimitedColor=BLACK;
        isStoppedColor=OTHER;
        isFilledColor=WHITE;
        maxLimitedNum=maxLimitedNum_black;
    }else{
        return;
    }

    for(int i=0;i<oriImg.rows-1;++i)
    {
        for(int j=1;j<oriImg.cols-1;++j)
        {
            int prePixelValue=(int)oriImg.at<uchar>(i,j-1);
            int curPixelValue=(int)oriImg.at<uchar>(i,j);
            if(colorType == WHITE)
            {
                if(prePixelValue == WHITE || curPixelValue!=curLimitedColor)
                    continue;
                unordered_map<int,bool> tempIsVisited;
                ConnectedEdge * pEdge=BFS(oriImg,Point2D(j,i),curLimitedColor,tempIsVisited,isStoppedColor,maxLimitedNum,true);
                if(pEdge!=nullptr)
                {
                    //cout<<"points.size(): "<< pEdge->points.size() <<endl;
                    list<Point2D>::iterator iter;
                    for(iter=pEdge->points.begin();iter!=pEdge->points.end();++iter)
                    {
                        optImg.at<uchar>(iter->y,iter->x)=isFilledColor;
                    }
                    delete pEdge;
                }
            }
            if(colorType == GRAY || colorType == BLACK)
            {
                if(prePixelValue!=preLimitedColor || curPixelValue!=curLimitedColor)
                    continue;
                unordered_map<int,bool> tempIsVisited;
                ConnectedEdge * pEdge=BFS(oriImg,Point2D(j,i),curLimitedColor,tempIsVisited,isStoppedColor,maxLimitedNum,true);
                if(pEdge!=nullptr)
                {
                    //cout<<"points.size(): "<< pEdge->points.size() <<endl;
                    list<Point2D>::iterator iter;
                    for(iter=pEdge->points.begin();iter!=pEdge->points.end();++iter)
                    {
                        optImg.at<uchar>(iter->y,iter->x)=isFilledColor;
                    }
                    delete pEdge;
                }
            }
        }
    }
}

void MapFilter_Zwk::removeIsolatedBlackPoint(Mat &oriImg, Mat &optImg)
{
    int blacks=0,grays=0,whites=0;
    for(int i=0;i<oriImg.rows;i++)
    {
        for(int j=0;j<oriImg.cols;j++)
        {
            int pixelValue=(int)oriImg.at<uchar>(i,j);
            if(pixelValue==BLACK)
            {
                for(int m=-1;m<2;m++)
                {
                    for (int n=-1; n < 2; n++)
                    {
                        if(m==0 && n==0)
                            continue;
//                        if(i+m <0 || i+m>oriImg.rows || j+n <0 || j+n>oriImg.cols)
//                        {
//                          int p_test=(int)oriImg.at<uchar>(i+m,j+n);
//                          ROS_WARN("p_test: %d",p_test);
//                        }

                        int p_value=(int)oriImg.at<uchar>(i+m,j+n);
                        if(p_value==BLACK)
                            ++blacks;
                        else if(p_value==WHITE){
                            ++whites;
                        }else
                        {
                            ++grays;
                        }
                    }
                }
            }
            if(blacks<1 && grays<whites)
            {
                optImg.at<uchar>(i,j)=WHITE;
            }
            blacks=0;whites=0;grays=0;
        }
    }
}

void MapFilter_Zwk::removeInvalidBlackPoint(Mat &oriImg, Mat &optImg)
{
    int blacks=0,grays=0,whites=0;
//    bool find_white = false;
    for(int i=0;i<oriImg.rows;i++)
    {
        for(int j=0;j<oriImg.cols;j++)
        {
            int pixelValue=(int)oriImg.at<uchar>(i,j);
            if(pixelValue==BLACK)
            {
                for(int m=-1;m<2;m++)
                {
                    for (int n=-1; n < 2; n++)
                    {
                        if(m==0 && n==0)
                            continue;
                        int p_value=(int)oriImg.at<uchar>(i+m,j+n);
                        if(p_value==BLACK){
                            ++blacks;
                        }
                        else if(p_value==WHITE){
                            ++whites;
//                            find_white =true;
//                            break;
                        }else{
                            ++grays;
                        }
                    }
//                    if(find_white)
//                    {
//                        find_white =false;
//                        break;
//                    }
                }
                if( whites<1 )
                {
                    optImg.at<uchar>(i,j)=GRAY;
                }
                blacks=0;whites=0;grays=0;
            }

        }
    }

}
void MapFilter_Zwk::removeWitheCircleGray(Mat &oriImg, Mat &binaryImg, Mat &optImg)
{
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(binaryImg,contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_NONE);

    vector<vector<Point>> gray_contours(contours);

    for(int i=1;i<contours.size();i++)//i=0为图像边缘最外层轮廓，应排除
    {
        //contours[i]代表的是第i个轮廓，contours[i].size()代表的是第i个轮廓上所有的像素点数
        for(int j=0;j<contours[i].size();j++)
        {
            for(int m=-inflate_num;m<inflate_num+1;m++)//轮廓膨胀
            {
                for(int n=-inflate_num;n<inflate_num+1;n++)
                {
                    if(m==0 && n==0)
                        continue;
                    Point P=Point(contours[i][j].x+n,contours[i][j].y+m);
                    gray_contours[i].push_back(P);
                }
            }
        }
    }

    vector<int> index;
    bool find_black = false;

    for(int i=0;i<gray_contours.size();i++)
    {
        double area = contourArea(contours[i], false);
        if(area < min_area || area > max_area )
            continue;
        for(int j=0;j<gray_contours[i].size();j++)
        {
            int pixelValue=(int)oriImg.at<uchar>(gray_contours[i][j].y,gray_contours[i][j].x);
            if(pixelValue==BLACK)
            {
                find_black =true;
                break;
            }
        }
        if(!find_black)
            index.push_back(i);
        find_black = false;
    }
    //cout<<"灰色轮廓数："<<index.size()<<endl;

    //填充灰色区域
    for(int i=0;i<index.size();i++)
    {
        drawContours(optImg,contours,index[i],Scalar(255,255,255),CV_FILLED,8,hierarchy);//CV_FILLED = -1,轮廓内部被填充
    }
}
void MapFilter_Zwk::runMapFilter(Mat &oriImg, Mat &optImg)
{
    if( oriImg.empty() )
    {
        cerr<<"origin Image load failed!"<<endl;
        return ;
    }
    if( oriImg.channels() != 1 )
    {
        cerr<<"origin Image channels must be one"<<endl;
        return ;
    }
    Mat M0=oriImg.clone();
    printf("0");

    for(int i=0;i<oriImg.rows;++i)
    {
        for(int j=0;j<oriImg.cols;++j)
        {
            int pixelValue=(int)oriImg.at<uchar>(i,j);
            if(pixelValue>=50&&pixelValue<240)
                oriImg.at<uchar>(i,j)=GRAY;
            else{
                if(pixelValue>=240)
                    oriImg.at<uchar>(i,j)=WHITE;
                else {
                    oriImg.at<uchar>(i,j)=BLACK;
                }
            }
        }
    }

    if(maxLimitedNum_black>0)
    {
        removeLittleColorRegion(M0,M0,BLACK);
        removeIsolatedBlackPoint(M0,M0);
    }
    printf("1");
    removeLittleColorRegion(M0,M0,WHITE);
    removeInvalidBlackPoint(M0,M0);
    printf("2");
    Mat binaryImg = M0.clone();
    threshold(M0,binaryImg,225,255,THRESH_BINARY);

    removeWitheCircleGray(M0,binaryImg,M0);
    printf("3");
    removeLittleColorRegion(M0,M0,GRAY);
    printf("4");

    optImg=M0.clone();
}